#include <Servo.h>

//byte startByte;
//int angle1;
//int angle2;
//int servo1pin = 9; // use a PWM pin
//int servo2pin = 10; // use a PWM pin
//int userInput[2] = {10, 10};
//Servo servo1;
//Servo servo2;
//
//void setup()
//{
//  Serial.begin(9600); // init serial
////  servo1.attach(servo1pin); // init servo
////  servo2.attach(servo2pin); // init servo
//}

#define BAUDRATE 9600
#define ROBOT_COUNT  2
#define FIRST_ROBOT_PIN 9

Servo Robots[ROBOT_COUNT];  // create an array of servos

void setup()	    
{
  for(int i=0; i < ROBOT_COUNT;i++)  
    Robots[i].attach(FIRST_ROBOT_PIN + i);  // attaches the servo to the servo object
  Serial.begin(BAUDRATE);
}


// these values store commands from the serial port
// the loop logic assumes the command format is:  servNbr,servoValue\n

int value = 0;
int servoNbr = 0;


void loop()
{
//  for (int i = 0; i < 181; i = i + 30)
//  {
//  servo1.write(i);
//  servo2.write(180-i);
//  delay(5000);
//  }
//  if (Serial.available() > 5) { // if we have enough data to start with
//      
//    startByte = Serial.read();  // we can start 'consuming' the buffer
//       if (startByte == 1) {  // check if we get the right sequence of data
//         for (int i=0; i<2; i++) {  
//           userInput[i] = Serial.read();  // now we read the data and make it re-usable
//         }  
//       
//         servo1.write(userInput[0]);  // so we can send it to the servo...
//         //Serial.write(userInput[0]);      // ...or back to the serial connection, perhaps for debugging
//       }
//       else if (startByte == 2) 
//       {
//         for (int i=0; i<2; i++) {  
//           userInput[i] = Serial.read();  // now we read the data and make it re-usable
//         }  
//         servo2.write(userInput[1]);  
//         //Serial.write(userInput[1]);
//      }
//    
//    delay(50);
//  }

  char c;    // holds one character from the serial port

  if (Serial.available()) {
    c = Serial.read();	// read one character

    //Serial.print(c);//write raw data to serial for testing

    if(c >= '0' && c <= '9'){		  // is ch a number?  
	value = value * 10 + c - '0';	     // yes, accumulate the value  
    }
    else if (c == ',') {    // if its comma then the preceeding value is the servo number
	servoNbr = value;
	//Serial.println(servoNbr);  // for debug
	value = 0;
    }
    else if (c == '!') {   //is it the end of message marker
	servoNbr--;  // the servo array starts at 0, use this if you send the first servo as number 1
	if(servoNbr >= 0 && servoNbr <  ROBOT_COUNT){
	  Robots[servoNbr].write(value);
          Serial.print(" wrote to servo: " );  // for debug	  
          Serial.print(servoNbr);  // for debug	  
          Serial.print(" value: " );  // for debug	            
	  Serial.println(value);  // for debug
	}
	else {
          Serial.println();
	  Serial.print(" got message for invalid servo number " );  // for debug	  
	  Serial.println(servoNbr);
	}
	value = 0;  
	Serial.println(" eol");    // debug marker
    }
  }
}

